﻿from node_red_msg_reader import node_red_msg_reader
from node_red_mpu6050_reader import mpu6050_i2c
import Hobot.GPIO as GPIO
import sys, time
import json
import atexit  # 用于在程序退出时自动调用某些函数
import threading
import queue

# 获取topic
topic = sys.argv[1]

exit_state = False

# 一些参数

#消息队列
msg_queue = queue.Queue()

# PID参数
Kp = 0.9
Ki = 0.05
Kd = 0.005

# 初始化PID变量
prev_error = 0.0
integral = 0.0
dt = 0.05  # 采样时间50ms

# 初始Yaw角度为0
yaw_set_point = 0.0
current_yaw = 0.0
pid_active = False  # 控制PID是否激活的标志位  临时  后续使用fsm控制计划删除

# gpio口
left_fw = sys.argv[2]
left_bw = sys.argv[3]
right_fw = sys.argv[4]
right_bw = sys.argv[5]

# 内部构建fsm驱动pid控制分支逻辑
current_state = "Stop"
pre_state = "Stop"
LEFT_PWM_PIN = int(left_fw)
LEFT_GPIO_PIN = int(left_bw)
RIGHT_PWM_PIN = int(right_fw)
RIGHT_GPIO_PIN = int(right_bw)



base_speed = 80

debug_record = ""


# 状态过滤器
state_filter = {
    "Stop": None,
    "Forward": None,
    "Backward": None,
    "Left": None,
    "Right": None
}

state_change = False


left_pwm = None
right_pwm = None

def gpio_init_setting():
    global left_pwm,right_pwm
    # GPIO初始化
    GPIO.setmode(GPIO.BOARD)

    GPIO.setup(LEFT_GPIO_PIN, GPIO.OUT, initial=GPIO.LOW)
    GPIO.setup(RIGHT_GPIO_PIN, GPIO.OUT, initial=GPIO.LOW)

    left_pwm = GPIO.PWM(LEFT_PWM_PIN, 48000)  # PWM频率48kHz
    right_pwm = GPIO.PWM(RIGHT_PWM_PIN, 48000)



    # 初始状态设置
    left_pwm.ChangeDutyCycle(1)
    right_pwm.ChangeDutyCycle(1)
    left_pwm.start(0)
    right_pwm.start(0)


def set_state(state):
    global current_state, pre_state,state_change

    # 过滤重复状态
    if state == current_state:
        return
        # 过滤无效状态
    if state in state_filter:
        pre_state = current_state
        current_state = state
        print("状态更新→" + state)
        #标记状态进行了有效变化
        state_change = True

def state_change_pid_init(state):
    #基本要读取当前的yaw值  然后需要定义一个基础的speed作为参数  如果是左转或者右转应该有其他的参数
    #v1版本以直行为基础服务对象
    global yaw_set_point,base_speed,current_yaw,integral

    integral = 0
    if  state == "Forward":
        yaw_set_point = mpu_tool.read_gyroscope(dt)
        current_yaw = yaw_set_point
        base_speed = 80
    elif state == "Backward":
        yaw_set_point = mpu_tool.read_gyroscope(dt)
        current_yaw = yaw_set_point
        base_speed = 80 #待定
    elif state == "Left":
        print("TODO")
    elif state == "Right":
        print("TODO")
    else:
        print("TODO")
    


# 定义消息解析函数
def msg_process(msg_data):
    # 假设消息格式为：{"state":"Forward",其他待定}
    try:
        msg_dict = json.loads(msg_data)  # 将 JSON 字符串解析为 Python 字典
        # 将json化的消息转递到消息队列中
        msg_queue.put(msg_dict)
    except json.JSONDecodeError:
        print("Failed to decode JSON message.")
    except KeyError:
        print("The key 'state' was not found in the message.")




def game_engine_like_process():
    #全局变量声明
    global current_state,pre_state,state_change
    
    
    while True:
        #消息队列读取
        if not msg_queue.empty():
            msg_dict = msg_queue.get()
            msg_state = msg_dict['state']
            # 状态变更逻辑
            set_state(msg_state)

        #PID初始化相关
        if state_change:
            state_change = False
            #相关状态PID的状态切换变量处理
            state_change_pid_init(current_state)
        
        #PID常规处理流
        normal_pid_process()
        
        #采样间隔
        time.sleep(dt)
        


def pid_forward():
   
    
    global prev_error, integral, current_yaw,yaw_set_point,current_state
    # print("状态处理curYaw:{0}".format(current_yaw))
    # 读取当前Yaw角度
    yaw_rate = mpu_tool.read_gyroscope(dt)
    #小于0.03度变化的数据剔除
    if  yaw_rate < 0:
        pass
    elif yaw_rate<0.04:
        yaw_rate = 0
    # print("yaw_rate:{0}".format(yaw_rate))
    current_yaw += yaw_rate
    
    # 计算误差
    error = yaw_set_point - current_yaw
    # print("current_yaw:{0}".format(current_yaw))
    # 计算PID输出
    integral += error * dt
    derivative = (error - prev_error) / dt
    output = Kp * error + Ki * integral + Kd * derivative
    prev_error = error
    # print("output:{0}".format(output))
  
    # # 根据PID输出调整轮速
    if output > 0:
        cal_left = min(100, base_speed + output)
        cal_right = max(70, base_speed - output)
        left_pwm.ChangeDutyCycle(cal_left)
        GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
        right_pwm.ChangeDutyCycle(cal_right)
        GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
        # print("left:{0}  right{1}".format(cal_left,cal_right))
        # print("右偏")
    elif output < 0:
        cal_left = max(70, base_speed + output)
        cal_right = min(100, base_speed - output)
        left_pwm.ChangeDutyCycle(cal_left)
        GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
        right_pwm.ChangeDutyCycle(cal_right)
        GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
        # print("left:{0}  right{1}".format(cal_left,cal_right))
        # print("左偏")
    else:
        # 直行
        left_pwm.ChangeDutyCycle(base_speed)
        GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
        right_pwm.ChangeDutyCycle(base_speed)
        GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
    # print("直行")
    
    
    

def pid_backward():

    # print("后退")
    left_pwm.ChangeDutyCycle(0)
    GPIO.output(LEFT_GPIO_PIN, GPIO.HIGH)
    right_pwm.ChangeDutyCycle(0)
    GPIO.output(RIGHT_GPIO_PIN, GPIO.HIGH)

def pid_left():

    # print("左转")
    left_pwm.ChangeDutyCycle(0)
    GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
    right_pwm.ChangeDutyCycle(base_speed)
    GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)

def pid_right():

    # print("右转")
    left_pwm.ChangeDutyCycle(base_speed)
    GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
    right_pwm.ChangeDutyCycle(0)
    GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)
    
def pid_stop():
    # print("停止")
    left_pwm.ChangeDutyCycle(0)
    GPIO.output(LEFT_GPIO_PIN, GPIO.LOW)
    right_pwm.ChangeDutyCycle(0)
    GPIO.output(RIGHT_GPIO_PIN, GPIO.LOW)



def normal_pid_process():
  
    if current_state == "Forward":
        pid_forward()
    elif current_state == "Backward":
        pid_backward()
    elif current_state == "Left":
        pid_left()
    elif current_state == "Right":
        pid_right()
    elif current_state == "Stop":
        pid_stop()
    else:
        print("非正常状态")
        


# 正式处理逻辑**



# step1:初始化gpio接口  后续可能转移到外部
gpio_init_setting()
print("Step1完成")

# step2:建立消息传递对象
msg_tool = node_red_msg_reader(topic, msg_process)





def start_mqtt_loop():
    msg_tool.connect()


# 创建并启动一个线程来运行 MQTT 循环
mqtt_thread = threading.Thread(target=start_mqtt_loop)
mqtt_thread.start()

print("Step2完成")

# step3：建立mpu读取工具
mpu_tool = mpu6050_i2c()
print("Step3完成")

# step4:为fsm的state指定对应的pid控制程序
# state_filter["Stop"] = None
# state_filter["Forward"] = None
# state_filter["Backward"] = None
# state_filter["Left"] = None
# state_filter["Right"] = None
print("开始程序内部loop处理")



# step5: 启动循环处理PID控制逻辑
game_engine_like_process()
print("系统运行结束")


@atexit.register
def cleanup():
    print("Cleaning up resources...")
    msg_tool.client.loop_stop()  # 停止 MQTT 监听
    mqtt_thread.join()  # 等待 MQTT 线程结束
    GPIO.cleanup()
